﻿#define DEBUG_DRAW

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using PriorityQueue;
using System;
using System.Runtime.InteropServices;
using System.Linq;


namespace PathFinding
{
    public class AStarPathFinder : IPathFInder
    {

        //Debug Draw
        public List<Vector2Int> OpenedPoints { get; private set; }
        public List<Vector2Int> OtherClosedPoints { get; private set; }

        // end debug draw

        public List<Vector2Int> CurrentPath { get; private set; }

        /// <summary>
        /// 可以在单位行动时逐步处理
        /// </summary>
        public List<Vector2Int> SmoothedPath { get; private set; }

        PFGrid grid;
        bool[] closed;
        PFNode[,] nodes;
        
        public bool IsWorking { get; private set; }

        public AStarPathFinder(PFGrid pFGrid)
        {
            grid = pFGrid;
            closed = new bool[grid.XSize * grid.YSize];

            nodes = new PFNode[grid.XSize, grid.YSize];

            for (int x = 0; x < grid.XSize; ++x)
            {
                for (int y = 0; y < grid.YSize; ++y)
                {
                    nodes[x, y] = new PFNode(new Vector2Int(x, y), grid[new Vector2Int(x, y)]);
                }
            }

            IsWorking = false;

            // debug draw
            OpenedPoints = new List<Vector2Int>(1024);
            OtherClosedPoints = new List<Vector2Int>(1024);

        }

        private void CleanLocalList()
        {
            Array.Clear(closed, 0, closed.Length);
            //Array.Clear(opened, 0, opened.Length);
            CurrentPath = null;
        }

        private void SetClosed(in Vector2Int pos)
        {
            closed[pos.x + pos.y * grid.XSize] = true;
        }

        private bool IsClosed(in Vector2Int pos)
        {
            return closed[pos.x + pos.y * grid.XSize];
        }

        public bool FindPath(Vector2Int start, Vector2Int goal, bool postSmooth = true)
        {
            if (IsWorking)
                throw new Exception("Can't init a new path finding now");
#if DEBUG_DRAW
            OpenedPoints.Clear();
            OtherClosedPoints.Clear();
#endif

            System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
            sw.Start();

            IsWorking = true;
            CleanLocalList();

            PFNodeHeap openHeap = new PFNodeHeap();
            PFNode startNode = nodes[start.x, start.y];
            startNode.gCost = 0;
            startNode.hCost = 0;
            startNode.parent = null;
            openHeap.Add(startNode);

#if DEBUG_DRAW
            OpenedPoints.Add(startNode.pos);
#endif

            while (openHeap.Count != 0)
            {
                PFNode nodeToVisit = openHeap.Pop();

                /// we found path
                if (nodeToVisit.pos == goal)
                {
                    sw.Stop();
                    Debug.Log("PathFind总共花费." + sw.Elapsed.TotalMilliseconds + " ms");


                    CurrentPath = new List<Vector2Int>(2);
                    while (nodeToVisit.parent != null)
                    {
                        CurrentPath.Add(nodeToVisit.pos);
                        nodeToVisit = nodeToVisit.parent;
                    }
                    CurrentPath.Add(start);

                    CurrentPath.Reverse();

                    // post smooth
                    if (postSmooth)
                    {
                        SmoothedPath = new List<Vector2Int>(2);

                        SmoothedPath.Add(CurrentPath.First());
                        var cs = CurrentPath[0];
                        for (var i = 1; i < CurrentPath.Count - 1; ++i)
                        {
                            if (!grid.IsLineOfSight(cs,CurrentPath[i + 1]))
                            {
                                SmoothedPath.Add(CurrentPath[i]);
                                cs = CurrentPath[i];
                            }
                        }

                        SmoothedPath.Add(CurrentPath.Last());
                    }
                    Debug.Log("Open: " + OpenedPoints.Count + " Closed: " + OtherClosedPoints.Count);

                    IsWorking = false;
                    return true;
                }

                SetClosed(nodeToVisit.pos);

                foreach (var npos in grid.pFPoints[nodeToVisit.pos.x, nodeToVisit.pos.y].Neighbors)
                {
                    if (!IsClosed(npos))
                    {
                        var adjacent = nodes[npos.x, npos.y];
                        if (adjacent.point.IsBlocked)
                        {
                            SetClosed(npos);
#if DEBUG_DRAW
                            OtherClosedPoints.Add(npos);
#endif
                            continue;
                        }

                        PFNode fromNode = nodeToVisit;

                        // Theta *
                        //{
                        //    if (nodeToVisit.parent != null && nodeToVisit.parent.IsLineOfSight(adjacent, nodes))
                        //        fromNode = nodeToVisit.parent;
                        //}

                        int toAdjacentCost = fromNode.gCost + NeighborDistCost(fromNode, adjacent);
                        if (!openHeap.Contains(adjacent) || toAdjacentCost < adjacent.gCost)
                        {
                            adjacent.gCost = toAdjacentCost;
                            adjacent.hCost = PredictDistanceCost(npos, goal);
                            adjacent.parent = fromNode;

                            if (openHeap.Contains(adjacent))
                                openHeap.UpdateItem(adjacent);
                            else
                            {
#if DEBUG_DRAW
                                OpenedPoints.Add(adjacent.pos);
#endif
                                openHeap.Add(adjacent);
                            }
                        }
                    }
                }

            }

            CurrentPath = null;
            SmoothedPath = null;
#if DEBUG_DRAW
            OpenedPoints.Clear();
            OtherClosedPoints.Clear();
#endif
            IsWorking = false;

            sw.Stop();
            Debug.Log("PathFind总共花费." + sw.Elapsed.TotalMilliseconds + " ms");

            return false;
        }


        private int NeighborDistCost(PFNode start, PFNode end)
        {
            int dx = Mathf.Abs(start.pos.x - end.pos.x);
            int dy = Mathf.Abs(start.pos.y - end.pos.y);

            return end.point.Roughness * (Mathf.Abs(dx - dy) * 10 + (dx == dy ? dy : 0) * 14);
        }

        /// <summary>
        /// 比g值计算方式略大，用以加速
        /// </summary>
        const int HValueMul = 13;
        /// <summary>
        /// 比g值计算方式略大，用以加速
        /// </summary>
        const int HValueMulDiagonal = 18;

        /// <summary>
        /// H 用于计算启发Cost 
        /// 即h值
        /// </summary>
        static int PredictDistanceCost(Vector2Int start, Vector2Int end)
        {
            //return Math.Abs(start.x - end.x) + Math.Abs(start.y - end.y); // 曼哈顿

            //return (int)((start - end).magnitude * 10); // 直线



            // 八向距离
            int dx = Mathf.Abs(start.x - end.x);
            int dy = Mathf.Abs(start.y - end.y);

            return Mathf.Abs(dx - dy) * HValueMul + (dx > dy ? dy : dx) * HValueMulDiagonal;

        }

        void IPathFInder.UpdateMap()
        {
            
        }
    }


}